#include "FireWithVision.h"
#include "FireShooterSensor.h"

FireWithVision::FireWithVision() {
        // Add Commands here:
        // e.g. AddSequential(new Command1());
        //      AddSequential(new Command2());
        // these will run in order.

        // To run multiple commands at the same time,
        // use AddParallel()
        // e.g. AddParallel(new Command1());
        //      AddSequential(new Command2());
        // Command1 and Command2 will run in parallel.

        // A command group will require all of the subsystems that each member
        // would require.
        // e.g. if Command1 requires chassis, and Command2 requires arm,
        // a CommandGroup containing them would require both the chassis and the
        // arm.
	
	turning = new TurnAngle(0, TURNANGLETOLERENCE, TURNANGLEDELTA, TURNANGLEZONE, COMPENSATION, 1.0);
#ifdef CHANGE_VA
	raise = new RaiseAngle(0 ,0.001);
	lower = new LowerAngle(100.0, 0.001);
#endif /* CHANGE_VA */
	speed = new StartShooterMotors(0, 3);
	
	AddSequential(turning);
#ifdef CHANGE_VA
	AddSequential(lower);
	AddSequential(raise);
#endif 
	AddSequential(speed);
	AddSequential(new FireShooterSensor());
	
}

void FireWithVision::Initialize(){
	double d, ha, va, s;
	int ret = Robot::targetingControl->vc->GetState(&ha, &va, &d, &s);
	//cout << "FireWithVision::Initialize: ret = " << ret << endl;
	//cout << "FireWithVision::Initialize: ha = " << ha << endl;
	//cout << "FireWithVision::Initialize: va = " << va << endl;
	//cout << "FireWithVision::Initialize: d = " << d << endl;
	//cout << "FireWithVision::Initialize: s = " << s << endl;
	if (ret == 0) {
		//cout << "Setting ha = " << ha << endl;
		turning->SetAngle(ha);
#ifdef CHANGE_VA
		double current_va_pot = Robot::shooterElevator->getPotPID();
		double pot_va = va * ((206.0 - 88.0) / (42.6 - 9.3));
		//cout << "Setting va = " << current_va_pot - pot_va << endl;
		
		raise->Set(current_va_pot - pot_va);
		lower->Set(100);
#endif
		//cout << "Setting s = " << s << endl;
		speed->Set(s);
	} else {
		// when we set all to 0 we effectively disable all commands
		turning->SetAngle(0);
#ifdef CHANGE_VA
		lower->Set(0);
		raise->Set(0);
#endif
		speed->Set(0);		
	}
}
